import cv2
import numpy as np
import matplotlib.pyplot as plt
import math

image_index = -1  # 图像序号
maxIndex = 12 # 文件夹下最大的图像张数
image_size = None  # 图像的尺寸（像素尺寸）
board_size = (8, 11)  # 棋盘格每行、列的角点数（不是棋盘格行列个数）
image_corners = []  # 每幅图像上检测到的角点数组
all_corners = []  # 所有图像角点数组
filePath = "E://test//pic_img//"  # 文件夹路径
real_size = 30.0 #图片上相邻角点的实际距离15mm

cell_size = (30.0, 30.0)  # 每个棋盘格的物理尺寸大小
object_points = []  # 棋盘格角点的三维坐标数组
imgpoints = []  # 保存二维点

#img=cv2.imread('00.jpg')#提取像素用
mat=[]#提取的像素点
filePath2="00c.jpg"
worldcoordinate=[[]]
imagein = cv2.imread("E://test//pic_img//22/00_c.jpg")
# 输入像素坐标原点
u0_image = 320 # 用实际的图像坐标替换
v0_image = 240  # 用实际的图像坐标替换
#输入想要标记的像素坐标
u_image = 340 # 用实际的图像坐标替换
v_image = 340  # 用实际的图像坐标替换
#每格像素实际物理尺寸
dx = 0.014
dy = 0.014
#相机到世界坐标系要做的刚体变换
R = np.array([[500, 200, 0],
              [500, 300, 0],
              [0, 0, 1]])
T = np.array([100, 200, 0])
#相机坐标系转齐次方程
V= np.zeros((4,1))


cameraMatrix = np.zeros((3, 3), dtype=np.float32)  # 摄像头内参数矩阵
distCoeffs = np.zeros((1, 5), dtype=np.float32)  # 摄像头5个畸变系数：k1, k2, p1, p2, k3
tvecsMat = []  # 图像的旋转向量数组
rvecsMat = []  # 图像的平移向量数组

# 在此处循环加载和处理图像，找到角点并填充all_corners
# 假设您在循环中读取每个图像并找到它们的角点
# for image_corners in all_corners:
#     rvecs = np.zeros((1, 3), dtype=np.float32)
#     tvecs = np.zeros((1, 3), dtype=np.float32)
#     ret, rvecs, tvecs = cv2.solvePnP(np.array(object_points), np.array(image_corners),
#                                     cameraMatrix, distCoeffs, rvecs, tvecs)
#     tvecsMat.append(tvecs)
#     rvecsMat.append(rvecs)
    
def addIndexToName(index, fileDirectory, tail):#文件路径
    filename = f"{filePath}/{fileDirectory}//{index:02d}{tail}"
    return filename

def addIndexToName1(index, fileDirectory, tail):#文件路径
    name = f"{fileDirectory}//Test{index:02d}{tail}"
    return name

def getImageCorners():#获得图像亚像素角点
    print("开始提取角点......")
    global image_index, image_size, all_corners,imageInput
    while True:
        image_index += 1
        if image_index >= maxIndex:
            break
        imageInput = cv2.imread(addIndexToName(image_index, "22", ".jpg"))
        if image_index == 0:
            image_size = (imageInput.shape[1], imageInput.shape[0])

        ret, image_corners = cv2.findChessboardCorners(imageInput, board_size, None)  
        if not ret:
            print("can not find chessboard corners in", addIndexToName(image_index, "22", ".jpg"), "!\n")
            exit(1)
        else:
            image_gray = cv2.cvtColor(imageInput, cv2.COLOR_BGR2GRAY)
            cv2.cornerSubPix(image_gray, image_corners, (11, 11), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))
            all_corners.append(image_corners)
            cv2.drawChessboardCorners(imageInput, board_size, image_corners, False)
            #cv2.imshow("Camera Calibration", imageInput)
            cv2.imwrite(addIndexToName(image_index, "22", "_a.jpg"), imageInput)
            cv2.waitKey(1)
    print("角点提取完成!")

def cameraCalibrate():#摄像头标定，得出内参外参
    print("开始标定......")
    global object_points, all_corners, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat

    for t in range(image_index):
        position = []
        for i in range(board_size[1]):
            for j in range(board_size[0]):
                realPoint = np.array([i * cell_size[0], j * cell_size[1], 0.0], dtype=np.float32)
                position.append(realPoint)
        object_points.append(position)

    rms, cameraMatrix, distCoeffs, rvecsMat, tvecsMat = cv2.calibrateCamera(np.array(object_points), all_corners, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0)
    print("标定完成!")

def outputCalibrateResult():#将标定结果保存到文件
    print("开始保存标定结果...")
    with open("result.txt", "w") as fout:
        fout.write("摄像头内参矩阵：\n")
        fout.write(str(cameraMatrix) + "\n\n")
        fout.write("摄像头畸变系数：\n")
        fout.write(str(distCoeffs) + "\n\n\n")
        
        print("/**********************/",image_index)
        for i in range(image_index):
            rotation_matrix, _ = cv2.Rodrigues(rvecsMat[i])
            fout.write(f"第 {i + 1} 幅图像的旋转矩阵：\n")
            fout.write(str(rotation_matrix) + "\n")
            fout.write(f"第 {i + 1} 幅图像的平移向量：\n")
            fout.write(str(tvecsMat[i]) + "\n\n")

    print("保存完成!")

def undistortImages():#矫正图像
    print("开始矫正图像...")
    mapx, mapy = cv2.initUndistortRectifyMap(cameraMatrix, distCoeffs, None, cameraMatrix, image_size, cv2.CV_32FC1)
    for i in range(image_index):
        imageInput = cv2.imread(addIndexToName(i, "22", ".jpg"))
        imageCorrect = cv2.remap(imageInput, mapx, mapy, cv2.INTER_LINEAR)
        cv2.imwrite(addIndexToName(i, "22", "_b.jpg"), imageCorrect)
    print("矫正结束")

def reProject():#生成每张图像的鸟瞰图
    print("开始生成鸟瞰图...")
    board_w = board_size[0]
    board_h = board_size[1]
    
    for i in range(image_index):
        h = np.zeros((3, 3), dtype=np.float32)
        objPts = []
        imgPts = []
        indexArray = [0, board_w - 1, (board_h - 1) * board_w, (board_h - 1) * board_w + board_w - 1]
        
        for j in range(4):
            objPt = np.array([object_points[i][indexArray[j]][0] * 2 + 100, object_points[i][indexArray[j]][1] * 2 + 100], dtype=np.float32)
            imgPt = all_corners[i][indexArray[j]]
            objPts.append(objPt)
            imgPts.append(imgPt)

        h = cv2.getPerspectiveTransform(np.array(objPts, dtype=np.float32), np.array(imgPts, dtype=np.float32))
        imageInput = cv2.imread(addIndexToName(i, "22", ".jpg"))
        birdImage = cv2.warpPerspective(imageInput, h, image_size, flags=cv2.INTER_LINEAR + cv2.WARP_INVERSE_MAP + cv2.WARP_FILL_OUTLIERS)
        cv2.imwrite(addIndexToName(i, "22", "_c.jpg"), birdImage)

    print("鸟瞰图生成结束")

def undistortSingleImages(i):#针对实际场景标定使用的棋盘格图像进行畸变矫正
    print("开始矫正单张图像...")
    mapx, mapy = cv2.initUndistortRectifyMap(cameraMatrix, distCoeffs, None, cameraMatrix, image_size, cv2.CV_32FC1)
    imageInput = cv2.imread(addIndexToName1(i, "pic", ".jpg"))
    imageCorrect = cv2.remap(imageInput, mapx, mapy, cv2.INTER_LINEAR)
    cv2.imwrite(addIndexToName1(i, "pic", "_b.jpg"), imageCorrect)
    print("矫正单张图像结束")

def getImageCorners1():#获取实际场景中标定使用的棋盘格的角点坐标，用于投影变换取值
    print("开始提取单个图像角点......")
    all_corners.clear()
    imageInput = cv2.imread(addIndexToName1(0, "pic", "_b.jpg"))
    # 提取角点
    image_gray = cv2.cvtColor(imageInput, cv2.COLOR_BGR2GRAY)
    ret, image_corners = cv2.findChessboardCorners(image_gray, board_size, None)  
    if not ret:
        print("can not find chessboard corners in", addIndexToName1(image_index, "pic", ".jpg"), "!")
        exit(1)
    else:
        cv2.cornerSubPix(image_gray, image_corners, (11, 11), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))
        all_corners.append(image_corners)
        cv2.drawChessboardCorners(imageInput, board_size, image_corners, True)
        cv2.imshow("Camera Calibration", imageInput)
        cv2.imwrite(addIndexToName1(0, "pic", "_c.jpg"), imageInput)
        cv2.waitKey(100)
    print("角点提取完成！")

def reProjectSingle(m):#针对实际场景需要测量的图像进行投影变换	
    print("开始生成单个鸟瞰图...")
    board_w = board_size[0]
    board_h = board_size[1]
    # 对每幅图像进行处理
    
    # 找到单应矩阵
    h = np.zeros((3, 3), dtype=np.float32)
    objPts = []
    imgPts = []
    i = 0
    # 每对顶点在顶点数组中的index
    indexArray = [0, board_w - 1, (board_h - 1) * board_w, (board_h - 1) * board_w + board_w - 1]
    # 给选定的4对顶点赋值：必须是point2f类型，所以objPts只取x, y坐标
    for j in range(4):
        objPt = np.array([object_points[i][indexArray[j]][0] * 2, object_points[i][indexArray[j]][1] * 2], dtype=np.float32)
        imgPt = all_corners[i][indexArray[j]]
        objPts.append(objPt)
        imgPts.append(imgPt)
    
    h = cv2.getPerspectiveTransform(np.array(objPts, dtype=np.float32), np.array(imgPts, dtype=np.float32))
    imageInput = cv2.imread(addIndexToName1(m, "pic", "_b.jpg"))
    birdImage = cv2.warpPerspective(imageInput, h, image_size, flags=cv2.INTER_LINEAR + cv2.WARP_INVERSE_MAP + cv2.WARP_FILL_OUTLIERS)
    cv2.imwrite(addIndexToName1(m, "pic", "_d.jpg"), birdImage)
    
    print("鸟瞰图生成结束")

def realsize():#像素和实际距离，不准
    imageInput = cv2.imread(addIndexToName(1, "pic", "_b.jpg"))
    print("imageInput is",imageInput)
    image_gray = cv2.cvtColor(imageInput, cv2.COLOR_BGR2GRAY)
    ret, image_corners = cv2.findChessboardCorners(image_gray, board_size, None) 
    board_w = board_size[0]
    board_h = board_size[1] 
    if ret:
        objp = np.zeros((board_w * board_h, 3), np.float32)
        objp[:, :2] = np.mgrid[0:board_w, 0:board_h].T.reshape(-1, 2) * real_size
        object_points.append(objp)
    #rms, cameraMatrix, distCoeffs, rvecsMat, tvecsMat = cv2.calibrateCamera(np.array(object_points), all_corners, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0)
    #ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(np.array(object_points), all_corners, image_size, mtx, dist,rvecs, tvecs, 0)
    undistorted = cv2.undistort(imageInput, cameraMatrix, distCoeffs, None, cameraMatrix)
    pixel_coords = np.array([[300, 210]], dtype=np.float32)
    #real_coords = cv2.undistortPoints(pixel_coords, cameraMatrix, distCoeffs)
    real_coords = (pixel_coords -(0,0))*0.5
    print("Pixel coordinates:", pixel_coords)
    print("Real-world coordinates:", real_coords)
    cv2.imwrite('undistorted.jpg', undistorted)

def Accuracy(filepath):#求解像素精度
    srcimg = cv2.imread(filepath)
    board_siz=[7,10]
    gray = cv2.cvtColor(srcimg, cv2.COLOR_BGR2GRAY)
    #print("gray:",gray)
    found, corners = cv2.findChessboardCorners(gray, board_siz, None)
    # print("found,corners:",corners)
    # print("检测到的原始角点坐标")
    # for i in range(len(corners)):
    #     print(f"第{i}个点（{corners[i][0][0]},{corners[i][0][1]})")

    if found:
        print("成功检测到角点")
        # criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        # cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        # cv2.drawChessboardCorners(srcimg, board_size, corners, False)
        # dstimg = cv2.resize(srcimg, (1000, 800))
        # cv2.imshow("jiaodian", dstimg)
        #cv2.waitKey(20)
        print("检测到的0.1亚像素斜的图像角点坐标")
        jingdux = 0
        totaljindu=0
        for i in range(len(corners)):
            if i == 0:#跳过第一个交点
                pass
            else:
                jingdux = np.sqrt((corners[i][0][0] - corners[i - 1][0][0]) ** 2 + (corners[i][0][1] - corners[i - 1][0][1]) ** 2)
                #print(jingdux)
                jingdux = 30.0 / jingdux
                totaljindu+=jingdux
            
            
        #     print(f"第{i}个点（{corners[i][0][0]},{corners[i][0][1]})")
        #     print(f"其像素精度为：{jingdux}/像素")
        # totaljindu=totaljindu/(len(corners))
        # print(f"其综合像素精度为：{totaljindu}/像素")
        return totaljindu

#回调函数
num=-1#用作计数
def on_EVENT_LBUTTONDOWN(event, x, y, flags, param):
    global num,mat
    if event == cv2.EVENT_LBUTTONDOWN:                                  #如果鼠标左键按下
        num+=1                                                          #标记点数加 1
        mat.append(x)                                                   #点坐标加入数组
        mat.append(y)
        xy = "%d,%d" % (x, y)                                           #将 x 和 y 坐标组合成字符串
        cv2.circle(imagein, (x, y), 2, (0, 0, 255), thickness = -1)       #在图像上画一个半径为 1 的圆点,BGR
        cv2.putText(imagein, xy, (x, y), cv2.FONT_HERSHEY_PLAIN,
                    1.0, (0,0,255), thickness = 1)
        print(mat[0+(2*num)],mat[1+(2*num)])
        cv2.imshow("image", imagein)

def display(imgsource):#标定像素点
    cv2.namedWindow("image")
    accurracy=0
    pixel=mat
    pixeldis=0
    realdis=0
    cv2.setMouseCallback("image", on_EVENT_LBUTTONDOWN) #设置鼠标回调函数为 on_EVENT_LBUTTONDOWN
    i = 0
    j = 0
    while(1):                                           # 进入循环，直到按下 ESC 键退出
        cv2.imshow("image", imgsource)
        if (cv2.waitKey() == 13) :                     #当enter键按下，且mat数组的个数是2的倍数时,且有新元素加入时
            if(num > -1) & ((num+1) % 2 == 0) & ((i*2) != len(pixel)):
                accurracy=Accuracy(filePath2)
                pixeldis=np.sqrt((pixel[2 + i]-pixel[0 + i])**2+(pixel[3 + i]-pixel[1 + i])**2)
                realdis=pixeldis*accurracy
                print("该两点间的距离为：(mm)",realdis)
                #redis = "distance between two point(mm):%f" % realdis
                #cv2.putText(imagein, redis, (20, 20), cv2.FONT_HERSHEY_PLAIN,1.0, (0,255,0), thickness = 1)
                i += 2
                continue
            else:
                print("再标记一个点")
                continue

        if (cv2.waitKey() == 27) or ( cv2.getWindowProperty("image", cv2.WND_PROP_VISIBLE) < 1):                   # 等待按键输入，如果是 ESC 键则退出循环
            break
    cv2.destroyAllWindows()

#临时定义的四个点
point1 = (175,319)
point2 = (341,319)
point3 = (341,399)
point4 = (175,399)

def drawrectangle():#在图片上画一个矩形
    image = imagein
    # 定义四个点坐标
    # 画矩形框
    cv2.line(image, point1, point2, (255, 0, 0), 2)
    cv2.line(image, point2, point3, (255, 0, 0), 2)
    cv2.line(image, point3, point4, (255, 0, 0), 2)
    cv2.line(image, point4, point1, (255, 0, 0), 2)

    # 显示画好矩形框的图像
    cv2.imshow("Rectangle", image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

def solve():#
    x=(point2[0]-point1[0])*Accuracy(filePath2)
    y=(point3[1]-point2[1])*Accuracy(filePath2)
   
    word_point=np.array([
    [[0,0,0],
    [x,0,0],
    [x,y,0],
    [0,y,0]
    ]], dtype=np.float32)

    pixpoint=np.array([  
    [[175,319],
    [341,319],
    [341,399],
    [175,399]
    ]], dtype=np.float32)

    cameraMatrix=np.array(
    [[473.9139868712908, 0, 321.7644494481638],
    [0, 473.6163661302777, 233.7803126852079],
    [0, 0, 1]
    ],dtype=np.double)

    distCoeffs=np.array([
        [0.044242 ,-0.0387607, -0.0024813 ,-0.0019008,
    0.01407]],dtype=np.double)
   
    imageInput=cv2.imread(filePath2)
    image_size = (imageInput.shape[1], imageInput.shape[0])
    #ret ,cameraMatrix, distCoeffs, rvecsMat, tvecsMat= cv2.calibrateCamera((word_point),pixpoint, image_size,None,None)
    success, rotationVector, translationVector = cv2.solvePnP(word_point, pixpoint, np.array(cameraMatrix), distCoeffs,flags=0)
    rotation_matrix, _ = cv2.Rodrigues(rotationVector)
    print(rotationVector)
    return rotation_matrix

def value():# 将旋转矩阵转换为欧拉角
    rotation_matrix=solve()
    # 假设 rotation_matrix 是您的旋转矩阵
    # 将旋转矩阵转换为欧拉角
    r = math.atan2(rotation_matrix[2, 1], rotation_matrix[2, 2])
    p = math.atan2(-rotation_matrix[2, 0], math.sqrt(rotation_matrix[2, 1] ** 2 + rotation_matrix[2, 2] ** 2))
    y = math.atan2(rotation_matrix[1, 0], rotation_matrix[0, 0])
    # 将弧度转换为角度
    r_angle = math.degrees(r)
    p_angle = math.degrees(p)
    y_angle = math.degrees(y)

    print("Roll (绕X轴旋转角度)：", r_angle)
    print("Pitch (绕Y轴旋转角度)：", p_angle)
    print("Yaw (绕Z轴旋转角度)：", y_angle)

#获取Zc的值，也就是相机相对于世界坐标系Z轴的值
def distance():
    fx = cameraMatrix[0][0]
    fy = cameraMatrix[1][1]
    cx = cameraMatrix[0][2]
    cy = cameraMatrix[1][2]
    imageInput = cv2.imread(addIndexToName(0, "22", ".jpg"))
    gray = cv2.cvtColor(imageInput,cv2.COLOR_BGR2GRAY)
    ret,get_corners = cv2.findChessboardCorners(gray,board_size,None)

    get_corners = cv2.cornerSubPix(gray, get_corners, (11, 11), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))
    cv2.drawChessboardCorners(imageInput,board_size,get_corners,False)
    chess_points = np.zeros((board_size[0] * board_size[1],3),np.float32)
    chess_points[:,:2] = np.mgrid[0:board_size[0],0:board_size[1]].T.reshape(-1,2) * real_size
    mtx = np.array([[fx,0,cx],[0,fy,cy],[0,0,1]])
    dist = np.array([0,0,0,0])
    ret,rvecs,tvecs = cv2.solvePnP(chess_points,get_corners,mtx,dist)
    distance = np.linalg.norm(tvecs)
    #print("Distance between camera and chessboard:",distance,"mm")
    #cv2.imshow("Chessboard",imageInput)
    #cv2.waitKey(10000)
    return distance

#/******************************************************************************/
#从像素点到世界坐标系的变换 

#循环取出矩阵里的每个参数
def iterate_matrix(matrix):
    parameters = []
    for row in matrix:
        for parameter in row:
            parameters.append(parameter)
    return parameters

def camera_to_world(R, T):
    # 创建一个4x4的单位矩阵
    world_matrix = np.eye(4)
    
    # 将旋转矩阵和平移向量填充到世界坐标系矩阵中
    world_matrix[:3, :3] = R
    world_matrix[:3, 3] = T
    
    return world_matrix

def multiply_martices(world_matrix,V):
    result = np.dot(world_matrix,V)
    return result

def pixel_to_camera():
    # 将像素坐标转换为校正后的像素坐标
    #undistorted_coords = cv2.undistortPoints(np.array([[u_image, u_image]], dtype=np.float32), cameraMatrix, distCoeffs)
    # 输出图像坐标
    print("像素坐标 (u, v)pixel:", u_image, v_image)
    x_image = (u_image - u0_image) * dx 
    y_image = (v_image - v0_image) * dy 
    print("图像坐标 (x, y)mm:", x_image, y_image)
    # 使用相机内参的逆来将校正后的像素坐标转换为相机坐标
    #camera_coords = np.dot(np.linalg.inv(cameraMatrix), np.array([undistorted_coords[0][0][0], undistorted_coords[0][0][1], 0.0]))
    # 输出相机坐标
    #x_camera = camera_coords[0]
    #y_camera = camera_coords[1]
    #z_camera = camera_coords[2]

    x_camera = (u_image - u0_image) * distance() / cameraMatrix[0][0]
    y_camera = (v_image - v0_image) * distance() / cameraMatrix[1][1]
    z_camera = distance()

    print("相机坐标 (x, y, z)mm:", x_camera, y_camera, z_camera)
    V[0,0] = x_camera
    V[1,0] = y_camera
    V[2,0] = 0
    V[3,0] = 1


    # 计算相机在世界坐标系下的坐标
    world_matrix = camera_to_world(R, T)
    result = multiply_martices(world_matrix,V)
    print("相机坐标系至世界坐标系要做的变换:\n", world_matrix)
    print("世界坐标系的齐次形式：\n",result)
    parameters = iterate_matrix(result)
    print("世界坐标系，齐次方程中的每个参数：\n",parameters)
    print("X坐标(单位mm)：",parameters[0])
    print("Y坐标(单位mm)：",parameters[1])
    print("Z坐标(单位mm)：",parameters[2])


if __name__ == "__main__":
    getImageCorners()
    #计算棋盘格实际尺寸
    cameraCalibrate()
    #print(cameraMatrix[0][0],"\n",distCoeffs)
    outputCalibrateResult()
    #undistortImages()
    reProject()

    #实际场景
    # undistortSingleImages(0)
    # getImageCorners1()
    # undistortSingleImages(1)
    # reProjectSingle(1)

    #Accuracy(filePath2)
    #measuredis(imagein)
    display(imagein)
    #drawrectangle()
    #solve()
    #value()

    #distance()
    #realsize()
    #pixel_to_camera()
